#include <cmath>
#include <iostream>
#include <string>
#include <cassert>
using namespace std;
#include "FlowSim.h"

using namespace FlowSim;
using namespace Ogre;
//0.00000713
MyFlowSim::MyFlowSim():_dt(0.5),
		       _dout(new TxtDebugOut()),
		       _vis(0.00000713),_wallSize(30),
		       _dim(200),
		       _sgr(1.0),
		       _dRate(0.90),
		       _playerX(200-10),
		       _playerY(200/2),
		       _playerDim(8),
		       _windSpeed(-0.0),
		       _brVF(false)
{
  _windForce.x = 0.0;
  _windForce.y = 0.0;
}

MyFlowSim::~MyFlowSim()
{
  delete _dout;
}

void MyFlowSim::init(SceneManager* sMgr)
{
  _scnMgr;
  _vfRender = new VectorField2Render(sMgr);
  _dyeRender = new DyeFieldRender(sMgr);
  _vf = VectorField2::blankField(_dim); //the simple test field
  _tvf = VectorField2::blankField(_dim); //get a blank field as our temp field.
  _psf = ScalarField2::blankField(_dim); //init the pressure scalar fields.
  _tpsf = ScalarField2::blankField(_dim);
  _divwsf = ScalarField2::blankField(_dim);
  _dyef = DyeField3::testField(_dim);
  _tdyef = DyeField3::blankField(_dim);
  //we project to make sure our inital velocity field is divergence free.
  boundaryVel(_vf);
  project();
  _dyeRender->setInput(_dyef.get());
  
  //_vfRender->setInput(_vf.get());
}
void MyFlowSim::resetSink(auto_ptr<VectorField2> vf,
			  auto_ptr<VectorField2> tvf,
			  auto_ptr<ScalarField2> psf,
			  auto_ptr<ScalarField2> tpsf,
			  auto_ptr<ScalarField2> divwsf,
			  auto_ptr<DyeField3> dyef,
			  auto_ptr<DyeField3> tdyef
			  )
{
}
void MyFlowSim::resetSink(auto_ptr<VectorField2> vf,
			  auto_ptr<VectorField2> tvf,
			  auto_ptr<ScalarField2> psf,
			  auto_ptr<ScalarField2> tpsf,
			  auto_ptr<ScalarField2> divwsf)
{
}
void MyFlowSim::reset()
{
  //reset auto_ptr's
  resetSink(_vf,_tvf,_psf,_tpsf,_divwsf);
  _vf = VectorField2::testField(_dim); //the simple test field
  _tvf = VectorField2::blankField(_dim); //get a blank field as our temp field.
  _psf = ScalarField2::blankField(_dim); //init the pressure scalar fields.
  _tpsf = ScalarField2::blankField(_dim);
  _divwsf = ScalarField2::blankField(_dim);
  //_dyef = DyeField3::testField(dim);
  //_tdyef = DyeField3::blankField(dim);
  //_vfRender->setInput(_vf.get());
}

/**
 *This method updates the simulation by dt.
 **/
void MyFlowSim::update()
{ 
  //calculate viscous diffusion
  jacobi(_vf,_tvf,_vf,1.0/(_vis*_dt),1/(4+(1.0/(_vis*_dt))),50);
  boundaryVel(_vf);
  //do projection
  project();
  
  advectVel(_vf,_tvf); //advect velocity field
  swap(_tvf,_vf); //swap the computed temp advected vector
  //do project again
  project();

  //advect dye
  sourceDye(_dyef);
  advectDye(_vf,_dyef,_tdyef);
  //boundaryDye(_dyef);
  _dyeRender->refresh(_dyef.get());
  //_vfRender->refresh(_vf.get()); //refresh vfRender with vector field vf.
}
void MyFlowSim::moveLeft()
{
  _playerX -= 1;
  if(_playerX < 1)
    _playerX = 1;
}
void MyFlowSim::moveRight()
{
  _playerX += 1;
  if(_playerX > _dim-2-_playerDim)
    _playerX = _dim-2-_playerDim;
}
void MyFlowSim::moveUp()
{
  _playerY += 1;
  if(_playerY > _dim-2-_playerDim)
     _playerY = _dim-2-_playerDim;
}
void MyFlowSim::moveDown()
{
  _playerY -= 1;
  if(_playerY < 1)
    _playerY = 1;
}
void MyFlowSim::changeSpeed(int speed)
{
  _windSpeed += speed;
}
void MyFlowSim::changeDifRate(float dr)
{
  _dRate += dr;
  if(_dRate > 1.0)
    _dRate = 1.0;
  else if(_dRate < 0.0)
    _dRate = 0.0;
}
void MyFlowSim::toggleVFRender()
{
  if(_brVF)
    _brVF = false;
  else
    _brVF = true;
}
/**
 *This method adds sources to the dye field.
 **/
void MyFlowSim::sourceDye(auto_ptr<DyeField3> &df)
{
  //add a source around left border
  // uint8* rd = df->getRData();
//   uint8* gd = df->getGData();
//   uint8* bd = df->getBData();
  Real* rd = df->getRData();
  Real* gd = df->getGData();
  Real* bd = df->getBData();
  int size = df->getSize();
  int sx1,sx2,sy1,sy2,sx3,sy3,sx4,sy4;
  int sDim = 4;
  sx1 = 0, sy1=size/2+sDim/2;
  sx2 = size-1-25; sy2 = 25;
  sx3 = size-1-25; sy3 = size-26;
  sx4 = size-1; sy4 = size/2+sDim/2;
  for(int i=0; i<sDim; i++)
    {
      for(int j=0; j<sDim; j++)
	{
	 //  for(int k=0; k<_vsx.size();k++)
// 	    {
// 	      sx = _vsx[k]; sy=_vsy[k];
// 	      if(sx > (size-1)-sDim)
// 		sx = (size-1)-sDim;
// 	      if(sy > (size-1)-sDim)
// 		sy = (size-1)-sDim;
// 	      rd[A_INDEX((sx+i),(sy+j),size)] = 0;
// 	      gd[A_INDEX((sx+i),(sy+j),size)] = 0;
// 	      bd[A_INDEX((sx+i),(sy+j),size)] = 255;
// 	    }
	  // rd[A_INDEX((sx1+i),(sy1-j),size)] = 1.0;
// 	  gd[A_INDEX((sx1+i),(sy1-j),size)] = 0;
// 	  bd[A_INDEX((sx1+i),(sy1-j),size)] = 0;

	  rd[A_INDEX((sx2+i),(sy2+j),size)] = 0;
	  gd[A_INDEX((sx2+i),(sy2+j),size)] = 0;
	  bd[A_INDEX((sx2+i),(sy2+j),size)] = 1.0;

	  rd[A_INDEX((sx2+i),(sy3-j),size)] = 0;
	  gd[A_INDEX((sx2+i),(sy3-j),size)] = 1.0;
	  bd[A_INDEX((sx2+i),(sy3-j),size)] = 0;

	  rd[A_INDEX((sx4-i),(sy4-j),size)] = 0.5;
	  gd[A_INDEX((sx4-i),(sy4-j),size)] = 0;
	  bd[A_INDEX((sx4-i),(sy4-j),size)] = 0.5;
	}
    }
  //player
  for(int i=0; i < _playerDim; i++)
    {
      for(int j=0; j < _playerDim; j++)
	{
	  rd[A_INDEX((_playerX+i),(_playerY+j),size)] = 1.0;
	  gd[A_INDEX((_playerX+i),(_playerY+j),size)] = 0.0;
	  bd[A_INDEX((_playerX+i),(_playerY+j),size)] = 0.0;
	}
    }
}

void MyFlowSim::addConstVel(auto_ptr<VectorField2> &vf)
{
  int size=vf->getSize();
  Vector2* vd = vf->getData();
  int ix =1; int iy=size/2;
  int wallSize = 10;
  Vector2 force(20.0,0.0);
  for(int i=0; i<wallSize;i++)
    {
      vd[A_INDEX(ix,(iy-i),size)] = force;
    }

}

void MyFlowSim::advectDye(auto_ptr<VectorField2> &vf,
			  auto_ptr<DyeField3> &df,
			  auto_ptr<DyeField3> &tdf)
{
  int size=vf->getSize();
  Vector2* data = vf->getData();
  // uint8* rd = df->getRData();
//   uint8* gd = df->getGData();
//   uint8* bd = df->getBData();
//   uint8* trd = tdf->getRData();
//   uint8* tgd = tdf->getGData();
//   uint8* tbd = tdf->getBData();
  Real* rd = df->getRData();
  Real* gd = df->getGData();
  Real* bd = df->getBData();
  Real* trd = tdf->getRData();
  Real* tgd = tdf->getGData();
  Real* tbd = tdf->getBData();
  Vector2 tv;
  Real rdx = 1.0/_sgr;
  //uint8 r,g,b;
  Real r,g,b;
  for(int i=0; i<size;i++)
    {
      for(int j=0;j<size;j++)
	{
	  tv.x = (Real)i; tv.y = (Real)j;
 	  //tv.x =(Real)i+0.5*_sgr; tv.y =(Real)j+0.5*_sgr;
	  bilerpDye(tv-_dt*rdx*data[A_INDEX(i,j,size)],r,
		    g,
		    b);
	  trd[A_INDEX(i,j,size)] = r*_dRate;
	  tgd[A_INDEX(i,j,size)] = g*_dRate;
	  tbd[A_INDEX(i,j,size)] = b*_dRate;
	}
    }
  swap(df,tdf);
}
/**
 *This method does the projection operation; after project we get a divergence free velocity field u.
 *We want to solve for a div free vector field u. By the Helmholtz decomposition,
 *u = w - Grad p
 *Now we know Laplacian p = Del . w
 *Since, w = u + Del p, now apply Divergence operator (Del .) to both side to get,
 * Del . w = Del . (u + Del p)
 * Del . w = Del . u + Del^2 p
 * But Del . u = 0, by the Navier-Stokes equation for incompressible fluids. So we get,
 * Del . w = Del^2 p
 * We can solve for p using Jacobi iteration.
 * Once we know p we can solve u using the formula given above.
 **/
void MyFlowSim::project()
{
  divergence(_vf,_divwsf);
  _psf->zeroField(); //set the inital pressure field to zero. Our inital guess with the Jacobi iteration will be zero's.
  jacobi(_psf,_tpsf,_divwsf,-1.0,0.25,70); //with alpha = -1.0, and beta = 1/4, 40 iterations.
  boundaryPre(_psf); //set the boundary conditions for scalar presuure field.
  computeDivFreeField(_vf,_psf,0.5); //the final step, u = w-del p.
  boundaryVel(_vf); //set the boundary for the velocity field again.
}
/**
 *This method carries out the advection on the velocity field with our stable method.
 **/
void MyFlowSim::advectVel(auto_ptr<VectorField2> &vf,
		       auto_ptr<VectorField2> &temp)
{
  //loop through the entire field. Remember we have a 1x1 sized boundary.
  int size = vf->getSize();
  Vector2* data = vf->getData();
  Vector2* tData = temp->getData();
  Vector2 tv;
  Real rdx = 1;
  for(int i = 1; i < size-1; i++)
    {
      for(int j = 1; j < size-1; j++)
	{
	  tv.x = (Real)i; tv.y = (Real)j;
	  //tv.x = (Real)i+0.5; tv.y = (Real)j + 0.5;
	  tData[A_INDEX(i,j,size)] = bilerp(vf,tv - _dt*rdx*data[A_INDEX(i,j,size)]);
	}
    }
  //handle boundary.
  boundaryVel(temp);
}
void MyFlowSim::testAdvectV()
{
  testAdvectVel(_vf,_tvf);
}
void MyFlowSim::testAdvectVel(auto_ptr<VectorField2> &vf,
			      auto_ptr<VectorField2> &temp)
{
  Real rdx = 1;
  int size = vf->getSize();
  Vector2* data = vf->getData();
  //test the first one.
  Vector2 tv,dv;
  tv.x = 1+0.5; tv.y = 1+0.5;
  dv = tv - _dt*rdx*data[A_INDEX(1,1,size)];
  //Vector2 f = bilerp(vf,dv);
  //_vfRender->addTestVec(dv,f);
}

void MyFlowSim::addForce(int x, int y, int width, int height)
{
  int size = _vf->getSize();
  Vector2* vd = _vf->getData();
  Vector2 force(1000.0,0.0);
  x = (int)((Real)x/width*size);
  y = (int)((Real)y/height*size);
  //cout << "x,y: " << (Real)x << " " << (Real)y << endl;
  Vector2 pos(x,y);
  Real r = 1000*size;
  Real temp;
  for(int i=1;i<size-1;i++)
    {
      for(int j=1;j<size-1;j++)
	{
	  //cout << "i,j: " << (Real)i << " " << (Real)j << endl;
	  temp = -((pos.x-i)*(pos.x-i)+(pos.y-j)*(pos.y-j));
	  //cout << "temp: " << temp << endl;
	  //cout << "exp(temp/r): " << exp(temp/r) << endl;
	  //cout << "dt*exp: " << _dt*exp(temp/r) << endl;
	  force *= _dt*exp(temp/r);
	  //cout << "force: " << force.x << " " << force.y << endl;
	  vd[A_INDEX(i,j,size)] += force;
	}
    }
  boundaryVel(_vf);
}

/**
 *Handles the boundary condition for a velocity field.
 *We want to set the bounary to the one negation of the value right next to it.
 **/
void MyFlowSim::boundaryVel(auto_ptr<VectorField2> &vf)
{
#ifdef DEBUG3
  cout << "--------------MyFlowSim::boundaryVel()-----------" << endl;
#endif
  int x=1;
  int y=1;
  int size = vf->getSize();
  Vector2* data = vf->getData();
  //set the 4 outer most corners
  //data[A_INDEX(0,0,size)] = Vector2::ZERO; //lower left corner
  //data[A_INDEX((size-1),0,size)] = Vector2::ZERO; //lower right corner
  //data[A_INDEX(0,(size-1),size)] = Vector2::ZERO; //upper left corner
  //data[A_INDEX((size-1),(size-1),size)] = Vector2::ZERO; //lower left corner
  //Vector2 t1,t2;
  int conSize = 8;

   //set up weird left constant veloity boundary (model wind tunnel.
  int iy = size/2+conSize;
  
  for(int i=0;i<size-2;i++)
    {

      data[A_INDEX((x+i),0,size)] = -data[A_INDEX((x+i),1,size)]; //lower border

      data[A_INDEX((x+i),(size-1),size)] = -data[A_INDEX((x+i),(size-2),size)]; //upper border

      data[A_INDEX(0,(y+i),size)] = -data[A_INDEX(1,(y+i),size)]; //left border
      //data[A_INDEX(0,(y+i),size)] = force; //left border
      data[A_INDEX((size-1),(y+i),size)] = -data[A_INDEX((size-2),(y+i),size)]; //right border
	  //_windForce.x = _windSpeed;
      //data[A_INDEX((size-1),(y+i),size)] = _windForce;
    }
  
  iy=size/2;
  _windForce.x = _windSpeed;
  for(int i=0; i<conSize;i++)
    {
      data[A_INDEX((size-1),(iy+i),size)] = _windForce; 
    }
  
//  int holesize = 6;
//  iy = size/2+holesize/2;
//     for(int i=0;i<_wallSize;i++)
//     {
//       data[A_INDEX(0,(iy-i),size)] = data[A_INDEX(1,(iy-i),size)];
////       data[A_INDEX(((size/2)+1),(iy-i),size)] = data[A_INDEX(((size/2)-2),(iy-i),size)];
//     }

  //draw a wall
   iy = size/2+_wallSize/2;
   for(int i=0;i<_wallSize;i++)
     {
       data[A_INDEX((size/2),(iy-i),size)] = -data[A_INDEX(((size/2)-1),(iy-i),size)];
       data[A_INDEX(((size/2)+1),(iy-i),size)] = -data[A_INDEX(((size/2)+2),(iy-i),size)];
     }
  
 //  iy = 1;
//   conSize=5;
//   force.x = 1.0; force.y = 5.0;
//   for(int i=0; i<conSize*2; i++)
//     {
//       data[A_INDEX(0,(iy+i),size)] = force;
//     }
#ifdef DEBUG3
  cout << "--------Borders after setting:-----------" << endl;
  for(int i=0;i<size-2;i++)
    {
      cout << "+++++++++++LOWER BORDER+++++++++++++++++++++++" << endl;
      cout << "x+i,0: " << x+i << " " << 0 << endl;
      t1 = data[A_INDEX(x+i,0,size)];
      cout << "t1: " << t1.x << " " << t1.y << endl;
      cout << "x+i,1: " << x+i << " " << 1 << endl;
      t2 = data[A_INDEX(x+i,1,size)];
      cout << "t2: " << t2.x << " " << t2.y << endl;
      cout << "+++++++++++Upper BORDER+++++++++++++++++++++++" << endl;
      cout << "x+i,size-1: " << x+i << " " << size-1 <<  endl;
      t1 = data[A_INDEX(x+i,size-1,size)];
      cout << "t1: " << t1.x << " " << t1.y << endl;
      cout << "x+i,size-2: " << x+i << " " << size-2 << endl;
      t2 = data[A_INDEX(x+i,1,size-2)];
      cout << "t2: " << t2.x << " " << t2.y << endl;
    }
#endif
  /*
  for(int i=0; i < size-2; i++)
    {
      data[A_INDEX(x+i,0,size)] = -data[A_INDEX(x+i,1,size)]; //lower border
#ifdef DEBUG3
      cout << "x,y: " << x+i << " " << 0 << endl;
      cout << "After setting border data:" << endl;
      cout << "lower corner: " << data[A_INDEX(x+i,0,size)].x << " " << 
	data[A_INDEX(x+i,0,size)].y << endl;
      cout << "above that: " << data[A_INDEX(x+i,1,size)].x << " " <<
	data[A_INDEX(x+i,1,size)].y << endl;
#endif
      //data[A_INDEX(x+i,size-1,size)] = -data[A_INDEX(x+i,size-2,size)]; //upper border
      data[A_INDEX(0,y+i,size)] = -data[A_INDEX(1,y+i,size)]; //left border
      //data[A_INDEX(size-1,y+i,size)] = -data[A_INDEX(size-2,y+i,size)];
      }*/

#ifdef DEBUG3
  cout << "----------Out boundaryVel-----------------" << endl;
#endif
}
/**
 *This method handles the boundary conditions for the pressure field.
 *We want to set the boundary to the value next to it.
 **/
void MyFlowSim::boundaryPre(auto_ptr<ScalarField2> &sf)
{
  int x=1;
  int y=1;
  int size=sf->getSize();
  Real* data=sf->getData();
  //4 corners
  data[A_INDEX(0,0,size)] = 0.0f; //lower left
  data[A_INDEX((size-1),0,size)] = 0.0f; //lower right
  data[A_INDEX(0,(size-1),size)] = 0.0f;
  data[A_INDEX((size-1),(size-1),size)] = 0.0f;
  
  for(int i=0; i < size-2; i++)
    {
      data[A_INDEX((x+i),0,size)] = data[A_INDEX((x+i),1,size)]; //lower border
      data[A_INDEX((x+i),(size-1),size)] = data[A_INDEX((x+i),(size-2),size)]; //upper border
      data[A_INDEX(0,(y+i),size)] = data[A_INDEX(1,(y+i),size)]; //left border
      data[A_INDEX((size-1),(y+i),size)] = data[A_INDEX((size-2),(y+i),size)];
      //data[A_INDEX((size-1),(y+i),size)] = data[A_INDEX(0,(y+i),size)]; 
    }
  //wall
   int iy = size/2+_wallSize/2;
   for(int i=0;i<_wallSize;i++)
     {
       data[A_INDEX((size/2),(iy-i),size)] = data[A_INDEX(((size/2)-1),(iy-i),size)];
       data[A_INDEX(((size/2)+1),(iy-i),size)] = data[A_INDEX(((size/2)+2),(iy-i),size)];
     }
}

void MyFlowSim::boundaryDye(auto_ptr<DyeField3> &df)
{
  int x=1;int y=1;
  int size=df->getSize();
  // uint8* rd = df->getRData();
//   uint8* gd = df->getGData();
//   uint8* bd = df->getBData();
  Real* rd = df->getRData();
  Real* gd = df->getGData();
  Real* bd = df->getBData();
  //4 corners
  rd[A_INDEX(0,0,size)] = 0; //lower left
  rd[A_INDEX((size-1),0,size)] = 0; //lower right
  rd[A_INDEX(0,(size-1),size)] = 0;
  rd[A_INDEX((size-1),(size-1),size)] = 0;
  
  gd[A_INDEX(0,0,size)] = 0; //lower left
  gd[A_INDEX((size-1),0,size)] = 0; //lower right
  gd[A_INDEX(0,(size-1),size)] = 0;
  gd[A_INDEX((size-1),(size-1),size)] = 0;

  bd[A_INDEX(0,0,size)] = 0; //lower left
  bd[A_INDEX((size-1),0,size)] = 0; //lower right
  bd[A_INDEX(0,(size-1),size)] = 0;
  bd[A_INDEX((size-1),(size-1),size)] = 0;
 
  for(int i=0; i < size-2; i++)
    {
      rd[A_INDEX((x+i),0,size)] = rd[A_INDEX((x+i),1,size)]; //lower border
      rd[A_INDEX((x+i),(size-1),size)] = rd[A_INDEX((x+i),(size-2),size)]; //upper border
      rd[A_INDEX(0,(y+i),size)] = rd[A_INDEX(1,(y+i),size)]; //left border
      rd[A_INDEX((size-1),(y+i),size)] = rd[A_INDEX((size-2),(y+i),size)];
      
      gd[A_INDEX((x+i),0,size)] = gd[A_INDEX((x+i),1,size)]; //lower border
      gd[A_INDEX((x+i),(size-1),size)] = gd[A_INDEX((x+i),(size-2),size)]; //upper border
      gd[A_INDEX(0,(y+i),size)] = gd[A_INDEX(1,(y+i),size)]; //left border
      gd[A_INDEX((size-1),(y+i),size)] = gd[A_INDEX((size-2),(y+i),size)];

      bd[A_INDEX((x+i),0,size)] = bd[A_INDEX((x+i),1,size)]; //lower border
      bd[A_INDEX((x+i),(size-1),size)] = bd[A_INDEX((x+i),(size-2),size)]; //upper border
      bd[A_INDEX(0,(y+i),size)] = bd[A_INDEX(1,(y+i),size)]; //left border
      bd[A_INDEX((size-1),(y+i),size)] = bd[A_INDEX((size-2),(y+i),size)];
    }
  
 //  //wall
//   int iy = size/2+_wallSize/2;
//   for(int i=0;i<_wallSize;i++)
//     {
//       rd[A_INDEX((size/2),(iy-i),size)] = rd[A_INDEX(((size/2)-1),(iy-i),size)];
//       rd[A_INDEX(((size/2)+1),(iy-i),size)] = rd[A_INDEX(((size/2)-2),(iy-i),size)];
//       gd[A_INDEX((size/2),(iy-i),size)] = gd[A_INDEX(((size/2)-1),(iy-i),size)];
//       gd[A_INDEX(((size/2)+1),(iy-i),size)] = gd[A_INDEX(((size/2)-2),(iy-i),size)];
//       bd[A_INDEX((size/2),(iy-i),size)] = bd[A_INDEX(((size/2)-1),(iy-i),size)];
//       bd[A_INDEX(((size/2)+1),(iy-i),size)] = bd[A_INDEX(((size/2)-2),(iy-i),size)];
}

/**
 *This method swaps v2 with v1. Where v1 and v2 are vector fields.
 **/
void MyFlowSim::swap(auto_ptr<VectorField2> &v1,
		     auto_ptr<VectorField2> &v2)
{
  auto_ptr<VectorField2> temp = v2;
  v2 = v1;
  v1 = temp;
}
/**
 *This method swaps v2 with v1. Where v1,v2 are vector fields.
 **/
void MyFlowSim::swap(auto_ptr<ScalarField2> &v1, auto_ptr<ScalarField2> &v2)
{
  auto_ptr<ScalarField2> temp = v2;
  v2 = v1;
  v1 = temp;
}

void MyFlowSim::swap(auto_ptr<DyeField3> &df1, auto_ptr<DyeField3> &df2)
{
  auto_ptr<DyeField3> temp = df2;
  df2 = df1;
  df1 = temp;
}

/**
 *This method linearly interpolates a 2D vector.
 *Such that,
 *y = y1 + (x-x1)*(y2-y1)/(x2-x1)
 **/
Vector2 MyFlowSim::lerp(Vector2 z1, Vector2 z2, Real x1, Real x2,Real x)
{
  return (z1 + ((x-x1)/(x2-x1))*(z2-z2)); 
}

void MyFlowSim::lerp(Real y1r, Real y1g, Real y1b,
		     Real y2r, Real y2g, Real y2b,
		     Real x1, Real x2, Real x,
		     Real &rr, Real &rg, Real &rb)
{
  Real t = (x-x1)/(x2-x1);
  rr = y1r + t*(y2r-y1r); 
  rg = y1g + t*(y2g-y1g);
  rb = y1b + t*(y2b-y1b);
  if(rr > 1.0)
    rr = 1.0;
  else if(rr < 0.0)
    rr = 0.0;
  if(rg > 1.0)
    rg = 1.0;
  else if(rg < 0.0)
    rg = 0.0;
  if(rb > 1.0)
    rb = 1.0;
  else if(rb < 0.0)
    rb = 0.0;
}
void MyFlowSim::bilerpDye(const Vector2 p, Real &r, Real &g, Real &b)
{
  Real tx,ty;
  Vector2 c11,c21,c12,c22;
  Vector2 pos = p;
  int size = _vf->getSize();
  int boffset = 2; //remember there is a 1x1 boundary. So (n-2) to (n-1) makes up this boundary.
                   //boffset = border offset
  Real loffset = 0.5;
  
  //test for conditions at boundary, so we don't go out of simulation grid.
  if(pos.x >= size-boffset + 0.5) //outside the cell on the right.
    {
      pos.x = size-boffset + 0.5; //set it to be on the boundary of the last cell.
      tx = (size-(boffset+1))+0.5; //the starting cell x location.
    }
  else if(pos.x < loffset) //outside the cell on the left
    {
      pos.x = loffset;
      tx = loffset;
    }
  else //inside the boundaries
    {
      tx = Math::Floor(pos.x-0.5) + 0.5;
    }
  if(pos.y >= size-boffset + 0.5)
    {
      pos.y = size-boffset + 0.5;
      ty = (size-(boffset+1))+0.5;
    }
  else if(pos.y < loffset)
    {
      pos.y = loffset;
      ty = loffset;
    }
  else
    ty = Math::Floor(pos.y-0.5)+0.5;
  //set the coords for the 4 nearest neighbors.
  c11.x = tx; c11.y = ty;
  c21.x = c11.x + 1.0; c21.y = c11.y;
  c12.x = c11.x; c12.y = c11.y + 1.0;
  c22.x = c11.x + 1.0; c22.y = c11.y + 1.0;

  //now bilinear interpolate.
  //Vector2* data = _vf->getData();
 //  uint8 *rd = _dyef->getRData();
//   uint8 *gd = _dyef->getGData();
//   uint8 *bd = _dyef->getBData();
  Real *rd = _dyef->getRData();
  Real *gd = _dyef->getGData();
  Real *bd = _dyef->getBData();
			
 //  uint8 tr1,tg1,tb1;
//   uint8 tr2,tg2,tb2;
//   uint8 tr3,tg3,tb3;
//   uint8 tr4,tg4,tb4;
  Real tr1,tg1,tb1;
  Real tr2,tg2,tb2;
  Real tr3,tg3,tb3;
  Real tr4,tg4,tb4;
  tr1 = rd[A_INDEX((int)c11.x,(int)c11.y,size)];
  tg1 = gd[A_INDEX((int)c11.x,(int)c11.y,size)];
  tb1 = bd[A_INDEX((int)c11.x,(int)c11.y,size)];
 
  tr2 = rd[A_INDEX((int)c21.x,(int)c21.y,size)];
  tg2 = gd[A_INDEX((int)c21.x,(int)c21.y,size)];
  tb2 = bd[A_INDEX((int)c21.x,(int)c21.y,size)];

  lerp(tr1,tg1,tb1,tr2,tg2,tb2,c11.x,c21.x,pos.x,
       tr3,tg3,tb3);
  
  tr1 = rd[A_INDEX((int)c12.x,(int)c12.y,size)];
  tg1 = gd[A_INDEX((int)c12.x,(int)c12.y,size)];
  tb1 = bd[A_INDEX((int)c12.x,(int)c12.y,size)];

  tr2 = rd[A_INDEX((int)c22.x,(int)c22.y,size)];
  tg2 = gd[A_INDEX((int)c22.x,(int)c22.y,size)];
  tb2 = bd[A_INDEX((int)c22.x,(int)c22.y,size)];

  lerp(tr1,tg1,tb1,tr2,tg2,tb2,c12.x,c22.x,pos.x,
       tr4,tg4,tb4);

  lerp(tr3,tg3,tb3,tr4,tg4,tb4,c11.y,c12.y,pos.y,
       r,g,b);
}
/**
 *This method bilinearly interpolates the vector field at position x,y.
 *
 * C12 ------------------- C22
 * |                       |
 * |         (x,y)         |
 * |                       |
 * C11 ------------------- C21
 *
 *The method works by finding the four nearest neighbors. Since we are working in the simulation domain,
 *what this means is that, the center of the cells form a rectangular region. The 4 known corners are
 *determined from Vector2 pos. Pos is the location where we landed. One can figure out from the code
 *how these 4 neighbors are determined. 
 *
 *We also check for boundary conditions, such that, if we land outside the rectangular region on the boundary
 *of the simulation grid, we move the position onto the edge of the boundary cell.
 **/
Vector2 MyFlowSim::bilerp(auto_ptr<VectorField2> &vf,const Vector2 p)
{
#ifdef DEBUG3
  cout << "MyFlowSim::bilerp()" << endl;
#endif 
  Real tx,ty;
  Vector2 c11,c21,c12,c22;
  Vector2 pos = p;
  int size = _vf->getSize();
  int boffset = 2; //remember there is a 1x1 boundary. So (n-2) to (n-1) makes up this boundary.
                   //boffset = border offset
  Real loffset = 0.5;
  //test for conditions at boundary, so we don't go out of simulation grid.
  if(pos.x >= size-boffset + 0.5) //outside the cell on the right.
    {
      pos.x = size-boffset + 0.5; //set it to be on the boundary of the last cell.
      tx = (size-(boffset+1))+0.5; //the starting cell x location.
    }
  else if(pos.x < loffset) //outside the cell on the left
    {
      pos.x = loffset;
      tx = loffset;
    }
  else //inside the boundaries
    {
      tx = Math::Floor(pos.x-0.5) + 0.5;
    }
  if(pos.y >= size-boffset + 0.5)
    {
      pos.y = size-boffset + 0.5;
      ty = (size-(boffset+1))+0.5;
    }
  else if(pos.y < loffset)
    {
      pos.y = loffset;
      ty = loffset;
    }
  else
    ty = Math::Floor(pos.y-0.5)+0.5;
  //set the coords for the 4 nearest neighbors.
  c11.x = tx; c11.y = ty;
  c21.x = c11.x + 1.0; c21.y = c11.y;
  c12.x = c11.x; c12.y = c11.y + 1.0;
  c22.x = c11.x + 1.0; c22.y = c11.y + 1.0;
#ifdef DEBUG3
  cout << "Size: " << size << endl;
  cout << "Pos: " << pos.x << " " << pos.y << endl;
  cout << "tx,ty: " << tx << " " << ty << endl;
  cout << "4 known points: " << endl;
  cout << "c11: " << c11.x << " " << c11.y << endl;
  cout << "c21: " << c21.x << " " << c21.y << endl;
  cout << "c12: " << c12.x << " " << c12.y << endl;
  cout << "c22: " << c22.x << " " << c22.y << endl;
#endif
  //now bilinear interpolate.
  Vector2* data = vf->getData();
#ifdef DEBUG3
  cout << "testing lerp l1" << endl;
  cout << "data[A_INDEX((int)c11.x,(int)c11.y,size)]: " << endl;
  Vector2 d1 = data[A_INDEX((int)c11.x,(int)c11.y,size)];
  cout << "d1: " << d1.x << " " << d1.y << endl;
  Vector2 d2 = data[A_INDEX((int)c21.x,(int)c21.y,size)];
  cout << "d2: " << d2.x << " " << d2.y << endl;
  Vector2 l = lerp(d1,d2,c11.x,c21.x,pos.x);
  cout << "l1: " << l.x << " " << l.y << endl;
  
  d1 = data[A_INDEX((int)c12.x,(int)c12.y,size)];
  d2 = data[A_INDEX((int)c22.x,(int)c22.y,size)];
  cout << "d3: " << d1.x << " " << d1.y << endl;
  cout << "d4: " << d2.x << " " << d2.y << endl;

  Vector2 l2 = lerp(d1,d2,c12.x,c22.x,pos.x);
  cout << "l2: " << l2.x << " " << l2.y << endl;
  
  Vector2 ret = lerp(l,l2,c11.y,c12.y,pos.y);
  cout << "ret: " << ret.x << " " << ret.y << endl;
  return ret;
#else
  Vector2 l1 = lerp(data[A_INDEX((int)(c11.x),(int)(c11.y),size)],
		    data[A_INDEX((int)(c21.x),(int)(c21.y),size)],
		    c11.x,c21.x,pos.x);
  Vector2 l2 = lerp(data[A_INDEX((int)(c12.x),(int)(c12.y),size)],
		    data[A_INDEX((int)(c22.x),(int)(c22.y),size)],
		    c12.x,c22.x,pos.x);
  
  return lerp(l1,l2,c11.y,l2.y,pos.y);
#endif
}


//------------------------------------------
/**
 *This method tests the bilinear interpolation by getting the interpolated vector plus it's
 *4 neighbors to da screen.
 **/
void MyFlowSim::testInterpolation(int x, int y, int wWidth, int wHeight)
{
  Vector2 pos;
  int size = _vf->getSize();
  //move into simulation domain
  pos.x = (Real)x/wWidth*size; 
  pos.y = (Real)y/wHeight*size;
#ifdef DEBUG3
  cout << "In testInterpolation:" << endl;
  cout << "X, Y: " << x << " " << y <<endl;
  cout << "sX, sY:" << pos.x << " " << pos.y << endl;
#endif
  Vector2 vec = bilerp(_vf,pos); //do the interpolation
#ifdef DEBUG3
   cout << "Vector has been interpolated:" << endl;
  cout << "V.x, V.y " << vec.x << " " << vec.y << endl;
#endif
  _vfRender->addTestVec(pos,vec); //render the interpolate vector and neighbors for test 
}

void MyFlowSim::addSource(int x, int y, int wWidth, int wHeight)
{
  int size = _vf->getSize();
  int sx = (int)((Real)x/wWidth*size);
  int sy = (int)((Real)y/wHeight*size);
  _vsx.push_back(sx);
  _vsy.push_back(sy);
}

/**
 *This method implements the Jacobi iteration. Use this method to solve Possion equations.
 *The parameter x and b are scalar fields.
 **/
void MyFlowSim::jacobi(auto_ptr<ScalarField2> &x, auto_ptr<ScalarField2> &tempx,
		       auto_ptr<ScalarField2> &b, Real alpha, Real rBeta,
		       int numIt)
{
#ifdef DEBUG3
  cout << "MyFlowSim::jacobi(\"the scalar field one\"" << endl;
  Real newx,tempb;
#endif
  Real xl,xr,yu,yd;
  int n=0;
  int size=x->getSize();
  assert(size==tempx->getSize() && "Jacobi iteration of Scalar fields: x and temp x dimension not equal!");
  assert(size==b->getSize() && "Jacobi iteration of scalar fields: x and b dimensions not equal!");
  Real* xnew;
  Real* xdata;
  Real* bdata = b->getData();
  while(n < numIt)
    {
      xnew = tempx->getData();
      xdata = x->getData();
#ifdef DEBUG3
      _dout->debugout(tempx,"tempx before setting.");
      _dout->debugout(x,"x before setting.");
      _dout->debugout(b,"b");
      cout << "+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++" << endl;
#endif
      for(int i=1; i<size-1;i++)
	{
	  for(int j=1; j<size-1; j++)
	    {
#ifdef DEBUG3
	      cout << i << "," << j << endl;
	      xl = xdata[A_INDEX((i-1),j,size)];
	      xr = xdata[A_INDEX((i+1),j,size)];
	      yd = xdata[A_INDEX(i,(j-1),size)];
	      yu = xdata[A_INDEX(i,(j+1),size)];
	      cout << "xl: " << xl << endl;
	      cout << "xr: " << xr << endl;
	      cout << "yd: " << yd << endl;
	      cout << "yu: " << yu << endl;
	      tempb = bdata[A_INDEX(i,j,size)];
	      cout << "tempb: " << tempb << endl;
	      newx = rBeta*(xl+xr+yd+yu+alpha*tempb);
	      cout << "newx: " << newx << endl;
	      xnew[A_INDEX(i,j,size)] = newx;
#else
	      //compute the 4 corners
	      xl = xdata[A_INDEX((i-1),j,size)];
	      xr = xdata[A_INDEX((i+1),j,size)];
	      yd = xdata[A_INDEX(i,(j-1),size)];
	      yu = xdata[A_INDEX(i,(j+1),size)];
	      xnew[A_INDEX(i,j,size)] = rBeta*(xl+xr+yd+yu+alpha*bdata[A_INDEX(i,j,size)]);
#endif
	    }
	}
#ifdef DEBUG3
      _dout->debugout(tempx,"tempx after setting.");
      _dout->debugout(x,"x after setting.");
      cout << "!!!!SWAPPING!!!" << endl;
#endif
      swap(x,tempx);
      ++n;
    }
}

/**
 *This method implements the Jacobi iteration.
 */
void MyFlowSim::jacobi(auto_ptr<VectorField2> &x, auto_ptr<VectorField2> &tempx,
		       auto_ptr<VectorField2> &b, Ogre::Real alpha, Ogre::Real rBeta,
		       int numIt)
{
  
  Vector2 xl,xr,yu,yd;
  int n = 0;
  int size = x->getSize();
  Vector2* xnew;
  Vector2* xdata;
  Vector2* bdata = b->getData();
  while(n < numIt) //do iterations
    {
      xnew = tempx->getData();
      xdata = x->getData();
      for(int i=1; i<size-1; i++)
	{
	  for(int j=1; j<size-1; j++)
	    {
	      //the 4 corners
	      xl = xdata[A_INDEX((i-1),j,size)];
	      xr = xdata[A_INDEX((i+1),j,size)];
	      yd = xdata[A_INDEX(i,(j-1),size)];
	      yu = xdata[A_INDEX(i,(j+1),size)];
	      xnew[A_INDEX(i,j,size)] = rBeta*(xl+xr+yd+yu+alpha*bdata[A_INDEX(i,j,size)]);
	    }
	}
      swap(x,tempx);
      n++;
    }
  
}
/**
 *This method computes the divergence of a vector field x, using finite difference.
 *We know Del . v = s. Where v is a vector field, and s is a scalar field.
 **/
void MyFlowSim::divergence(auto_ptr<VectorField2> &vf,
			   auto_ptr<ScalarField2> &sf)
{
#ifdef DEBUG3
  cout << "++++++MyFlowSim::divergence()+++++++" << endl;
#endif
  Real halfrdx = 0.5; //dx is 1 for now
  Vector2* vdata = vf->getData();
  Real* sdata = sf->getData();
  int size = vf->getSize();
  assert(size == sf->getSize() && "Computing divergence, but input vector field dimension is not that of the output scalar field!");
  Vector2 wl,wr,wu,wd;
#ifdef DEBUG3
  _dout->debugout(vf,"vector field w before divergence step.");
  _dout->debugout(sf,"temp scalar field ts before divergence step.");
#endif
  Vector2 tv;
  for(int i=1;i<size-1;i++)
    {
      for(int j=1;j<size-1;j++)
	{
// 	  tv.y = (Real)(j) + 0.5;
// 	  tv.x = (Real)(i-1)+0.5;
// 	  wl = tv + vdata[A_INDEX((i-1),j,size)];
// 	  tv.x = (Real)(i+1)+0.5;
// 	  wr = tv + vdata[A_INDEX((i+1),j,size)];
// 	  tv.x = (Real)(i)+0.5; tv.y = (Real)(j+1)+0.5;
// 	  wu = tv + vdata[A_INDEX(i,(j+1),size)];
// 	  tv.y = (Real)(j-1)+0.5;
// 	  wd = tv + vdata[A_INDEX(i,(j-1),size)];
// 	  sdata[A_INDEX(i,j,size)] = halfrdx*((wr.x-wl.x)+(wu.y-wd.y));


	  wl = vdata[A_INDEX((i-1),j,size)];
	  wr = vdata[A_INDEX((i+1),j,size)];
	  wu = vdata[A_INDEX(i,(j+1),size)];
	  wd = vdata[A_INDEX(i,(j-1),size)];
	  sdata[A_INDEX(i,j,size)] = halfrdx*((wr.x-wl.x)+(wu.y-wd.y));
	}
    }
#ifdef DEBUG3
  _dout->debugout(vf,"vector field w after divergence step.");
  _dout->debugout(sf,"temp scalar field after divergence step.");
  cout << "++++++++Finished MyFLowSim::divergence()+++++++++" << endl;
#endif
}
/**
 * By Hemholtz decomposition:
 * w = u + del p, where w is an arbitray vector field, it is a combination of
 * a solendial vector field plus the gradient of the pressure field p.
 * Thus, to compute a divergence free vector field we do:
 * u = w - del p.
 **/
void MyFlowSim::computeDivFreeField(auto_ptr<VectorField2> &w,
				    auto_ptr<ScalarField2> &p, Real halfrdx)
{
  int size = p->getSize();
  assert(size == w->getSize() && "Computing Div Free field: pressure field and velocity field dimension not same!");
  Real* sdata = p->getData();
  Vector2* vdata = w->getData();
  Vector2 temp;
  Real pl,pr,pu,pd;
  for(int i=1;i<size-1;i++)
    {
      for(int j=1;j<size-1;j++)
	{
	  //compute the 4 neighbors needed for the gradient finite difference formula
	  pl = sdata[A_INDEX((i-1),j,size)];
	  pr = sdata[A_INDEX((i+1),j,size)];
	  pu = sdata[A_INDEX(i,(j+1),size)];
	  pd = sdata[A_INDEX(i,(j-1),size)];
	  temp.x = pr-pl;
	  temp.y = pu-pd;
	  //subtract the gradident field from the velocity field w to get div free field u.
	  vdata[A_INDEX(i,j,size)].x -= halfrdx*temp.x;
	  vdata[A_INDEX(i,j,size)].y -= halfrdx*temp.y;
	}
    }
}
